/******************************************************************************
 * Copyright (C) 2023 Advanced Micro Devices, Inc. All Rights Reserved.
 * SPDX-License-Identifier: MIT
 ******************************************************************************/
/*
 * helloworld.c: simple test application
 *
 * This application configures UART 16550 to baud rate 9600.
 * PS7 UART (Zynq) is not initialized by this application, since
 * bootrom/bsp configures it to baud rate 115200
 *
 * ------------------------------------------------
 * | UART TYPE   BAUD RATE                        |
 * ------------------------------------------------
 *   uartns550   9600
 *   uartlite    Configurable only in HW design
 *   ps7_uart    115200 (configured by bootrom/bsp)
 */

#include "platform.h"
#include "xgpio.h"
#include "xil_printf.h"
#include "xparameters.h"
#include <stdio.h>
#include <xstatus.h>

#define LED_EQ_SW 1

int init_XGpio(XGpio *InstancePtr, UINTPTR BaseAddress);
void show_Led_and_Switch(u32 SwitchInput, u32 LedOutput);
void set_LedOutput(u32 *LedOutput, u32 SwitchInput);

int main() {
  init_platform();

  print("Hello World\n\r");
  print("Successfully ran Hello World application\n\r");

  // init XGPIO device
  XGpio Switch, Led;
  if (init_XGpio(&Switch, XPAR_DIP_SWITCH_GPIO_BASEADDR) != XST_SUCCESS) {
    return XST_FAILURE;
  }
  if (init_XGpio(&Led, XPAR_LED_GPIO_BASEADDR) != XST_SUCCESS) {
    return XST_FAILURE;
  }
  print("init completed!\r\n");

  // set direction for Led
  XGpio_SetDataDirection(&Led, 1, 0);
  XGpio_SetDataDirection(&Switch, 1, 1);

  // set the input and output status
  u32 LedOutput = 0, SwitchInput = 0;
  while (1) {
    SwitchInput = XGpio_DiscreteRead(&Switch, 1);
    set_LedOutput(&LedOutput, SwitchInput);
    show_Led_and_Switch(SwitchInput, LedOutput);
    XGpio_DiscreteWrite(&Led, 1, LedOutput);
  }

  cleanup_platform();
  return 0;
}

int init_XGpio(XGpio *InstancePtr, UINTPTR BaseAddress) {
  xil_printf("init XGPIO device with BaseAddress = %x\n\r", BaseAddress);
  XGpio_Config *Config = XGpio_LookupConfig(BaseAddress);
  return XGpio_CfgInitialize(InstancePtr, Config, Config->BaseAddress);
}

int get_bit(u32 num, size_t index) {
  // get the index bit of num
  return (num & (1 << index)) >> index;
}

void show_Status(u32 Status) {
  xil_printf(" status: %3d <==> %d%d%d%d ", Status, get_bit(Status, 0),
             get_bit(Status, 1), get_bit(Status, 2), get_bit(Status, 3));
  return;
}

void show_Led_and_Switch(u32 SwitchInput, u32 LedOutput) {
  static u32 previousSwitchInput = -1; // 上一次的开关输入状态
  static u32 previousLedOutput = -1;   // 上一次的LED输出状态

  if (SwitchInput == previousSwitchInput && LedOutput == previousLedOutput) {
    return;
  }

  print("Switch");
  show_Status(SwitchInput);
  print("Led");
  show_Status(LedOutput);
  print("\r\n");

  // 更新上一次的状态
  previousSwitchInput = SwitchInput;
  previousLedOutput = LedOutput;
  return;
}

void set_bit(u32 *num, size_t index, int value) {
  // Clear the bit at the given index
  *num &= ~(1 << index);

  // Set the bit at the given index with the new value
  *num |= (value << index);
}

void set_LedOutput(u32 *LedOutput, u32 SwitchInput) {
  if (LED_EQ_SW) {
    *LedOutput = SwitchInput;
  } else {
    // set led2=sw3 and led3=sw2
    set_bit(LedOutput, 3, get_bit(SwitchInput, 2));
    set_bit(LedOutput, 2, get_bit(SwitchInput, 3));
  }
  return;
}